#!/usr/bin/env python3
import os
import json
import cv2
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
from lane_detection.read_param import LaneDetectionConfig

class CameraOriNode(Node):
    """原始相机图像发布节点。
    
    该节点从左右相机读取原始图像，拼接后发布到ROS话题。
    
    Attributes:
        config (LaneDetectionConfig): 车道线检测配置参数。
        cap_L (cv2.VideoCapture): 左侧相机视频捕获对象。
        cap_R (cv2.VideoCapture): 右侧相机视频捕获对象。
        publisher (Publisher): ROS图像发布器。
        bridge (CvBridge): OpenCV与ROS图像转换桥。
        timer (Timer): 定时器用于控制发布频率。
    """
    
    def __init__(self):
        """初始化原始相机图像发布节点。
        
        加载配置参数，初始化视频捕获对象，创建发布者和定时器。
        """
        super().__init__('camera_ori_node')

        # 从YAML文件加载配置
        self.config = LaneDetectionConfig.from_yaml('src/lane_detection/config.yaml')
        LaneDetectionConfig.get_calibration_file(self.config)
        
        # 获取相机参数
        self.left_camera = self.config.camera.left_video
        self.right_camera = self.config.camera.right_video
        self.get_logger().info(f"左侧相机源: {self.left_camera}")
        self.get_logger().info(f"右侧相机源: {self.right_camera}")
        
        # 输出话题和发布频率
        output_topic = '/lane_detection/image_raw'
        publish_rate = 60.0
        
        # 检查视频源是否存在
        if not os.path.exists(self.left_camera) or not os.path.exists(self.right_camera):
            self.get_logger().error(f"视频源不存在: {self.left_camera} 或 {self.right_camera}")
            return

        # 初始化视频捕获对象
        self.cap_L = cv2.VideoCapture(self.left_camera)      # 左侧摄像头
        self.cap_R = cv2.VideoCapture(self.right_camera)     # 右侧摄像头
        
        # 检查视频捕获对象是否成功创建
        if not self.cap_L.isOpened() or not self.cap_R.isOpened():
            raise ValueError("无法创建VideoCapture，请检查视频源或摄像头连接。")
        
        # 创建发布者
        self.publisher = self.create_publisher(Image, output_topic, 2)
        self.bridge = CvBridge()
        
        # 创建定时器控制发布频率
        timer_period = 1.0 / publish_rate
        self.timer = self.create_timer(timer_period, self.timer_callback)
        
        self.get_logger().info(f'原始相机图像发布节点已初始化')
        self.get_logger().info(f'发布到: {output_topic} 频率: {publish_rate} Hz')

    def load_config(self, config_path: str) -> dict:
        """从JSON文件加载配置。
        
        Args:
            config_path (str): JSON配置文件路径。
            
        Returns:
            dict: 加载的配置字典。
            
        Raises:
            FileNotFoundError: 如果配置文件不存在。
            JSONDecodeError: 如果JSON解析失败。
        """
        try:
            # 检查文件是否存在
            if not os.path.isfile(config_path):
                self.get_logger().warn(f"配置文件不存在: {config_path}")
                return {}
            
            # 读取并解析JSON文件
            with open(config_path, 'r') as f:
                config = json.load(f)
            
            self.get_logger().info(f"成功加载配置文件: {config_path}")
            return config
            
        except Exception as e:
            self.get_logger().error(f"加载配置文件失败: {str(e)}")
            return {}

    def timer_callback(self):
        """定时器回调函数。
        
        读取左右相机图像，拼接后发布到ROS话题。
        """
        try:
            # 读取左右相机图像
            ret_L, frame_L = self.cap_L.read()
            ret_R, frame_R = self.cap_R.read()

            # 如果读取失败，重新初始化视频捕获对象
            if not ret_L or not ret_R:
                self.get_logger().warn("读取图像失败，重新初始化视频捕获对象")
                self.cap_L = cv2.VideoCapture(self.left_camera)
                self.cap_R = cv2.VideoCapture(self.right_camera)
                return
            
            # 拼接左右图像
            batched_frame = cv2.hconcat([frame_L, frame_R])

            # 转换为ROS图像消息并发布
            msg = self.bridge.cv2_to_imgmsg(batched_frame, encoding='bgr8')
            msg.header.stamp = self.get_clock().now().to_msg()
            msg.header.frame_id = "camera"
            self.publisher.publish(msg)
            
        except Exception as e:
            self.get_logger().error(f'发布图像时出错: {str(e)}')

    def release(self):
        """释放资源。
        
        释放视频捕获对象。
        """
        self.cap_L.release()
        self.cap_R.release()
        self.get_logger().info("视频捕获对象已释放")

def main(args=None):
    """主函数。
    
    初始化ROS 2，创建节点并运行。
    
    Args:
        args: 命令行参数。
    """
    rclpy.init(args=args)
    node = CameraOriNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.release()
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()